import math
import numpy as np
from utils.utils import *

if __name__ == "__main__":
    # dict = {1:[1,2,3], 2:[2,3,4], 3:{1:[4,5], 2:[6,7]}}
    # print(dict[1])
    # print(list((dict[1][i] - dict[2][i]) for i in range(1,3)))
    # print(dict[3][1])
    state_raw = {1: [1, 2910.0, 0.0, 30, 0.0, 0.0, False], 2: [2, -243.28862009203516, -158.99176646625415, 23.797483231352818, -1.74257991110885, 0.0, False], 3: [3, 714.8041231634108, -629.3102110994754, 16.98558328921578, -3.033384471220331, 0.0, False], 4: [4, -143.44562365652314, -165.9424213243835, 19.048627503957505, -0.007621095438856384, 0.0, False], 5: [5, -253.7990087503555, 417.13379537867456, 19.64623861929681, -1.096183493953989, 0.0, False]}
    vec2tar = state_raw[1][1:3]
    print(list([[3]]))
    # print(np.linalg.norm(vec2tar))
    # for key, value in dict.items():
    #     print(key, value)